/*
master.ino
Description:    Setup and Main loop for Arduino code

Author:         T Bristow

Date Created:   20/04/13 
Date Modified:  27/05/13
Revision:       8
*/

#include <Wire.h>
#include <HardwareSerial.h>
#include <Arduino.h>
#include "GPSSerial.h"
#include "MMGi2c.h"
#include "SoftSerial.h"
#include "multiplexer.h"
#include "packets.h"

#define SENSOR_COUNT 12

char packetData[4+SENSOR_COUNT*2+GPS_SENTENCE_LENGTH];      // array to store the content of a data packet, time+sensors+gps
unsigned long* timeStamp=(unsigned long*)packetData;        // pointer to time stamp in data packet content
unsigned short* sensorData=(unsigned short*)(packetData+4); // pointer to beginning of sensor section of data content
char* RMCPoint=packetData+(4+SENSOR_COUNT*2);                // pointer to beginning of GPS char array, array is filled by calling GPSparse()
Packet dataPacket;                                            // re-usable Packet structure for storing the data packet
Packet rxPacket;                                             // Packet structure for receiving a packet
unsigned long t0;

//Array containing the device configuration section
//each sensor is represented by 2 bytes, first is device type ID
//second is data format type ID
char DeviceConfig[]={
  SDID_ELYCRA,DFID_UINT16,
  SDID_ELYCRA,DFID_UINT16,
  SDID_FLEX,DFID_UINT16,
  SDID_FLEX,DFID_UINT16,
  SDID_THERMISTOR,DFID_UINT16,
  SDID_FORCE,DFID_UINT16,
  SDID_PULSE,DFID_UINT16,
  SDID_FORCE,DFID_UINT16,
  SDID_ACCX,DFID_UINT16,
  SDID_ACCY,DFID_UINT16,
  SDID_MMG,DFID_UINT16,
  SDID_MMG,DFID_UINT16,
  SDID_GPS,DFID_STRING
};

//Map of multiplexer channel number to position in data packet
char multiplexerConfig[8]={0,1,2,3,7,4,6,5};


void setup(){            
  Serial.begin(115200);        // Serial used for Bluetooth 
  initialiseGPS(RMCPoint);    // Initialise GPS/Software Serial reader, and set output to GPS sentence buffer in data packet content
  initialiseMMG();
  initialiseMultiplexer();

  dataPacket.StartPacket=PACKET_START;    // Set Packet start byte
  dataPacket.EndPacket=PACKET_END;        // Set Packet end byte
  dataPacket.MessageType=DATA_MESSAGE;  // set Packet type to Data Message
  dataPacket.DataContent=packetData;    // set data content pointer to buffer
  
  connect();
}

void connect(){ // Waits for a connection request from the PC and then connects
  Packet txPacket;                                        // Packet structure for sending a packet
  
  do{
    do{
      while(!readPacket(&rxPacket));                     // wait until a packet is received
    }
    while(rxPacket.MessageType!=INITIALISATION_REQUEST); // make sure it's an initialisation request message, otherwise wait for another packet
    txPacket.StartPacket=PACKET_START;                     // Initialise the Trasmit packet
    txPacket.EndPacket=PACKET_END;
    txPacket.MessageType=DEVICE_CONFIGURATION_STATUS;    // Set packet type to device configuration packet
    txPacket.DataLength=sizeof(DeviceConfig);
    txPacket.DataContent=DeviceConfig;                     // Set data content of packet to device configuration
    updateChecksum(&txPacket);
    sendPacket(&txPacket);                                 // Send device configuration packet to the PC
    while(!readPacket(&rxPacket));                         // Wait for a packet to  be received
  }
  while(rxPacket.MessageType!=MESSAGE_ACK);                 // Make sure it's an acknowledgement packet
  t0=millis();                                             // Record the start time
}

unsigned long lastLoopTime=0;

void loop(){
  while(millis()<lastLoopTime+50);                        // Wait until 50 ms have passed since the last measurement (limits to 20 samples/s) 
  char i;
  *timeStamp=millis()-t0;                                // set time stamp in ms to time since switch on.
  for(i=0;i<8;i++){                                        // step through multiplexer channels
    setMultiplexer(i);                                    // set active multiplexer channel
    delayMicroseconds(10);                                // wait 10 us for voltage to settle
    sensorData[multiplexerConfig[i]]=analogRead(A0);    // read sensor and store in sensor data section of packet
  }
  sensorData[8]=analogRead(A2);                            // read sensors that don't connect through multiplexer
  sensorData[9]=analogRead(A3);
  sensorData[10]=readMMG(MMG_ADDR_1);                    // get most recent MMG data
  sensorData[11]=readMMG(MMG_ADDR_2);
  GPSparse();                                            // check for new GPS data

  dataPacket.DataLength=4+2*SENSOR_COUNT+getGPSSentenceLength(); // re-calculate data length for data content
  updateChecksum(&dataPacket);
  sendPacket(&dataPacket);                                // send data packet to PC
  lastLoopTime=millis();                                // store time that data sent to limit rate to 20 samples/s

  if(readPacket(&rxPacket)){
    if(rxPacket.MessageType==DISCONNECT_REQUEST){        // if disconnect request received wait for a new connect request
      connect();
    }
  }
}